package org.scalpel.mainApps.HHObjects;

import java.text.DecimalFormat;
import java.text.NumberFormat;

import org.opencv.core.Core;
import org.opencv.core.Point;
//import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Scalar;

import org.scalpel.HHObjects.MapManager;
import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;

import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.Log;

import ioio.lib.api.AnalogInput;
import ioio.lib.api.IOIO;
import ioio.lib.api.exception.ConnectionLostException;
//import static java.lang.Math.*;

/**
 * @author Yvanne Paviet-Salomon
 * @version 1.0
 * Les Sharps quelque soit leurs coordonnées servent à mettre un point "blanc"
 * sur la map grâce à la classe Core et Mat [line et put].
 * solution: faire un stop si obstacle et relancer une batterie de trajectoire si abscence.
 * Pb: Homme, totem autour vont être considérer comme des obstacles,
 * Sharp non parfait, prévoir marge de securité pour retirer la detection
 * zone de diminution de detection hard car angle d'attaque de l'obstacle différent
 * en fct des sharps
 * discrétisation des sharps hard car systeme continue et non linéaire avec cône de détection
 * le Sharp doit donc etre déactiver quand il va commencer à voir les obstacles non intéressant
 * 
 **/


public class Sharp extends HHObject {

	
  private static final String TAG = "Sharp";
private double fDistanceDetection =0.25 ;
  private double error_distance=0.05 ;
  private double fDistanceDetection_with_error =error_distance + fDistanceDetection ;

  
  private double fDetectionObstacle =10;
  private double Dimension_on_X = 3; //(meter)
  private double Dimension_on_Y = 2;

  private boolean fIsSimulation = false;
	private int fPin = 0; 
	private IOIO fIoio = null;
	private KeyPoint fPosition_Sharp_start = new KeyPoint();
	private KeyPoint fPosition_Sharp_current = new KeyPoint();
	
	private AnalogInput fAnalogInput = null;

private double fMeterToPix;
  private KeyPoint fCurrPosition;
  
  //private Mat fArr = null;
  
  NumberFormat fNumberFormat;
  
  float fVoltage = 0.f;
  /**Constructor*/  
	public Sharp(String name, int pin, double px, double py, double pa, double meterToPix){
		super(name);
		
	  fPin = pin;
	  fMeterToPix = meterToPix;
	  fPosition_Sharp_start.x = px;
	  fPosition_Sharp_start.y = py;
	  fPosition_Sharp_start.a = pa; //Par rapport au centre du robot
	  /**CHoisir un autre repère?*/
	}

	protected void finalize() throws Throwable
	{
		if( fAnalogInput != null)
			fAnalogInput.close();
	  super.finalize(); //not necessary if extending Object.
	} 
	
	public void initIOIO(boolean isSimulation, IOIO ioio){
		fIsSimulation = isSimulation;

		if(fIsSimulation) return;
		
		fIoio = ioio;
		try {
			fAnalogInput = fIoio.openAnalogInput(fPin);
		} catch (ConnectionLostException e) {
			e.printStackTrace();
		};
		
	}
	
	public void updateIOIO(){
		

		if( fIsSimulation ) return;
		
		if( fIoio == null || 
				fAnalogInput == null ||
						fCurrPosition == null
				) return;
		
		
			try {
				fVoltage = fAnalogInput.getVoltage();
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			} catch (ConnectionLostException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}


			/**conversion V--> Distance*/
			
				//fDetectionObstacle = convertVolt2Meter(fVoltage);
			
			fPosition_Sharp_current.x = fPosition_Sharp_start.x + fCurrPosition.x; 
			fPosition_Sharp_current.y = fPosition_Sharp_start.y + fCurrPosition.y;
			
			//Log.v(TAG, "to**********updatelancer********tototo");
			/**DEBUT des conditions d'arrêt des Sharps*/
			/**Pour le bord--> 4 conditions d'exclusion*/
			/**Pour le centre--> 4 conditions d'exclusion*/
			/**A verifier*/
			
			
			//if(Math.abs( fDetectionObstacle )< 0.2 )
			//{
/*Log.v(TAG, "to**********updatelancer********tototo");
setChanged();
notifyObservers(new Object[]{"Stop_Robot", new Object[]{true,0}});*/
				//setChanged();
				//notifyObservers(new Object[]{"TestObstacle", new Object[]{true,false}});
			//} 
			//else {
				//setChanged();
				//notifyObservers(new Object[]{"TestObstacle", new Object[]{false,true}});
			//}
			/**Logique à revoir*/
			/*if (fPosition_Sharp_current.x<fDistanceDetection_with_error && fPosition_Sharp_current.y<fDistanceDetection_with_error )
			{
				if (Dimension_on_X - fPosition_Sharp_current.x<fDistanceDetection_with_error && Dimension_on_Y- fPosition_Sharp_current.y<fDistanceDetection_with_error )
					//on ne lit pas les sharps
				{
					
					if (fPosition_Sharp_current.x>0.8 && fPosition_Sharp_current.x <1.2 && fPosition_Sharp_current.y >1 && fPosition_Sharp_current.y<1.4)
					{
										if( fDetectionObstacle < 0.2 )
							{
			/*Log.v(TAG, "to**********updatelancer********tototo");
			setChanged();
			notifyObservers(new Object[]{"Stop_Robot", new Object[]{true,0}});*/
					/*						setChanged();
											notifyObservers(new Object[]{"StopRobot", new Object[]{0.,0.,0.}});
											Log.v(TAG, "to**********updatelancer********tototo");
											setChanged();
											notifyObservers(new Object[]{"StopRobot2", new Object[]{0.,0.,0.}});
											setChanged();
											notifyObservers(new Object[]{"StopRobot3", new Object[]{0.,0.,0.}});
							} 
					}
				}
			}
			else{ 
				setChanged();
				notifyObservers(new Object[]{"StopRobot", new Object[]{0.71,0.15,0}});
				Log.v(TAG, "to**********updatelancer********tototo");
				setChanged();
				notifyObservers(new Object[]{"StopRobot2", new Object[]{0.5,0.3,0.1}});
				setChanged();
				notifyObservers(new Object[]{"StopRobot3", new Object[]{6.,1.,0.1}});
				/*setChanged();
				  notifyObservers(new Object[]{"Go_Robot",null});*/
			/*}*/
	}
/** c'est modloco qui lance cette méthode par un evenement dans son updateioio*/
	public boolean TestShrap() throws InterruptedException, ConnectionLostException
	{
		
			fVoltage = fAnalogInput.getVoltage();
			fDetectionObstacle = convertVolt2Meter(fVoltage);
			
			fPosition_Sharp_current.x = fPosition_Sharp_start.x + fCurrPosition.x; 
			fPosition_Sharp_current.y = fPosition_Sharp_start.y + fCurrPosition.y;
			
			if (fPosition_Sharp_current.x>0.9 && fPosition_Sharp_current.x <1.9 && fPosition_Sharp_current.y >0.60 && fPosition_Sharp_current.y<1.4)
			{return false;}
			
			if(Math.abs( fDetectionObstacle )< 0.3 )
			{
			return true;
			}
			return false;
	}
	
	public void setCurrentPosition(KeyPoint currPos)
	{		
		fCurrPosition = currPos; // current position of the robot in the field space
	}
	/**Rp: deltaroue crée la variation de distance--> variation d'angle
	 * modification du repère général du robot (cf Odometry)
	 * il me faut prendre ce changement angulaire pour caractériser les
	 * sharps de manière unique, use probalmg?
	 */
	/** Afficher un cercle de présence du robot adverse si possible*/
	/**Il me faut la current pos et la distancedetection */
	/**Pb: cela dépendra de l'orientation des sharps donc du robot*/
	/**Il me faut l'angle!!! fCurrPos est un Keypoint, il le possède*/
	/** patcher le lance*/
	public void onDraw(Canvas canvas, Paint paint){
		if( fCurrPosition == null) return;
	    //canvas.drawCircle(connectedPosX, connectedPosY, 3, robotPaint);

		KeyPoint p = new KeyPoint();
		p.x = fPosition_Sharp_start.x; /**position fixe?*/
		p.y = fPosition_Sharp_start.y;
		p.a = fPosition_Sharp_start.a;
		/**	Rotation du point p1 autour du point centre*/
		/**le point au centre est recalculer à partir de ces info*/
		Point p1 = MapManager.rotationPoint(
				fPosition_Sharp_start.x + fCurrPosition.x, 
				fPosition_Sharp_start.y + fCurrPosition.y, 
				fCurrPosition, fCurrPosition.a, fMeterToPix);
		/**Pk que le x? oui car on a définit un angle de départ*/
		synchronized(this) {
			p.x += fDistanceDetection;
		}
		/** cf fct dans keypoint, deja use dans trajectory, add angle actuel au delta*/
		/** sans prise en compte de metertopix */
		/**Mais prise en compte de la variation angulaire*/
		/**son deuxieme point a un delta x en plus*/
		p.rotate(fPosition_Sharp_start, fPosition_Sharp_start.a);
		Point p2 = MapManager.rotationPoint(
				p.x + fCurrPosition.x, 
				p.y + fCurrPosition.y, 
				fCurrPosition, fCurrPosition.a, fMeterToPix);
		
		
		paint.setColor(Color.RED);
		/**tracer les papate des sharps*/
		canvas.drawLine((float)p1.x, (float)p1.y, (float)p2.x, (float)p2.y, paint);
		
		/*Affichage de la valeur de la distance*/
		/*paint.setColor(Color.BLACK);
		String posString = fNumberFormat.format(fDistanceDetection);
  	canvas.drawText(posString, (float)p2.x,(float) p2.y, paint);
  	posString = fNumberFormat.format(fVoltage);
  	canvas.drawText(posString, (float)p2.x,(float) p2.y+10, paint);
		 */

	}

	synchronized public double getDistanceDetected(){
		return fDistanceDetection;
	} 
/** map manager l'appel ds un update si absence obstacle */
	/**fParamDrawObstacle different de null à un moment?*/
	
	public void drawNoObstacleLine(Canvas canvas, Paint paint, Mat probaImg, int zoomFactor){
		if( fCurrPosition == null) return;

		//boolean foundSomething = false;
		KeyPoint p = new KeyPoint();
		p.x = fPosition_Sharp_start.x;
		p.y = fPosition_Sharp_start.y;
		p.a = fPosition_Sharp_start.a;
		Point p1 = MapManager.rotationPoint(
				fPosition_Sharp_start.x + fCurrPosition.x, 
				fPosition_Sharp_start.y + fCurrPosition.y, 
				fCurrPosition, fCurrPosition.a, fMeterToPix/zoomFactor);
		
		synchronized(this) {
			p.x += fDetectionObstacle;
		};
		p.rotate(fPosition_Sharp_start, fPosition_Sharp_start.a);
		Point p2 = MapManager.rotationPoint(
				p.x + fCurrPosition.x, 
				p.y + fCurrPosition.y, 
				fCurrPosition, fCurrPosition.a, fMeterToPix/zoomFactor);
		if(fIsSimulation){
			
		}
		Core.line(probaImg, p2, p2, new Scalar(0),//CvScalar.BLACK
								1, 
								16,//opencv_core.CV_AA    * CV_AA - antialiased line.
								0);
		if( !fIsSimulation && fDetectionObstacle <0.2){
			// TODO see if cvSetAt could be the solution
			probaImg.put((int)p2.y, (int)p2.x, 255.0/*WHITE*/); //met un point blanc.
			/** le jack repond a ces criteres malgres la diff de pin*/
		} 
		 
	}
	
    public double pow (double V, double Expo) {
        return java.lang.Math.pow(V,Expo);
    }
    
	private double convertVolt2Meter(double valueInVolt)
	{	
		/**<courbe polynomiale d'ordre 5:  d(cm) = f(V)*/
		/**Distance Information to enemy*/
		double distance;
		double V = valueInVolt;
		switch (fPin)
		{
		
		case 36:  distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		 break;
		case 37:  distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		break; 
		case 38:   distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		  //distance = (0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (86.166*V) - 53.21;  
			break; 
		case 39:  distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		break; 
		case 41:  distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		break;
		case 43:  distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		break; 
		case 42:  distance = (-0.5977 * pow(V,5)) + (6.7585 * pow(V,4)) +  (-30.529 * pow(V,3)) + (70.133 * pow(V,2)) + (-86.166*V) + 53.21;  
		break; 
		default:
		distance =19999; break; 
		}
		double distance_m = distance /100;
		return distance_m;
	}

	@Override
	public void finish() {
		// TODO Auto-generated method stub
		
	}
	
}
